#include "TSDF.cuh"
// #include <memory>

#include "main.h"
void SaveVoxelGrid2SurfacePointCloud(cv::Mat &p, cv::Mat &col, const std::string &file_name)
{

    FILE *fp = fopen(cv::format("%s.ply", file_name.c_str()).c_str(), "w");
    fprintf(fp, "ply\n");
    fprintf(fp, "format binary_little_endian 1.0\n");
    fprintf(fp, "element vertex %ld\n", p.rows);
    fprintf(fp, "property float x\n");
    fprintf(fp, "property float y\n");
    fprintf(fp, "property float z\n");
    fprintf(fp, "property uchar red\n");
    fprintf(fp, "property uchar green\n");
    fprintf(fp, "property uchar blue\n");
    fprintf(fp, "end_header\n");
    cout << file_name << ":" << p.rows << endl;
    for (int i = 0; i < p.rows; i++)
    {
        fwrite(p.ptr<cv::Vec3f>(i, 0), sizeof(float), 3, fp);
        fwrite(col.ptr<cv::Vec3b>(i, 0), sizeof(uchar), 3, fp);
    }
    fclose(fp);
}

kin::kin()
{
    ck(cudaGetLastError());
    cv::FileStorage fs("../MarkerSLAM/camera.yml", cv::FileStorage::READ);
    std::fstream fss("../2023_05_09_16_05_02/file.txt", std::ios::in);
    std::vector<string> as;
    for (int i = 0; i < 2000; ++i)
    {
        string s1;
        fss >> s1;
        as.push_back(" ../2023_05_09_16_05_02" + s1);
    }
    fss.close();

    if (!fs.isOpened())
    {
        // cout << " 没有" << datapath + "/info.yaml" << endl;
        assert(0);
    }
    int2 imgsz;
    Mat cam_K;

    fs["camera_matrix"] >> cam_K;
    fs["image_height"] >> imgsz.x;
    fs["image_width"] >> imgsz.y;
    float4 cam = make_float4(cam_K.at<float>(0, 0), cam_K.at<float>(1, 1), cam_K.at<float>(0, 2), cam_K.at<float>(1, 2));

    ptsdf = new TSDF(make_uint3(512, 512, 512), imgsz);
    ptsdf->pintr = new Camera(cam);
    ptsdf->pintr->sca = 1.0f / 1000.0f;
    cv::FileStorage fs3("./mpose4.yml", cv::FileStorage::READ);

    fs3["paths_rgbs"] >> paths_rgbs;
    fs3["paths_deps"] >> paths_deps;
    fs3["ALLpaths_rgbs"] >> ALLpaths_rgbs;

    fs3["pose"] >> poses;
    fs3["idx"] >> idx;
    fs3.release();

    pindx = idx.ptr<int>();
    pfsss = ALLpaths_rgbs.ptr<float>();

    // Init cvui and tell it to create a OpenCV window, i.e. cv::namedWindow(WINDOW_NAME).
}

void kin::runall()
{
    for (int i = 0; i < paths_rgbs.size(); i++)
    {
        // int j = pindx[i * 3];
        cout << paths_rgbs[i] << endl;
        cv::Mat rgb = cv::imread(paths_rgbs[i]);
        cv::Mat depth = cv::imread(paths_deps[i], -1);
        cout << depth.isContinuous() << endl;
        // assert(rgb.empty());
        // assert(depth.empty());
        cv::imshow("depth", depth);
        cv::imshow("rgb", rgb);
        cv::waitKey(0);

        cv::Affine3f pose(pfsss + i * 16);
        cout << pose.matrix << endl;
        // 读取图片
        // cv::Mat rgb = cv::imread(cv::format("%s/rgb/%d.png", path.c_str(), i));
        // cv::Mat depth = cv::imread(cv::format("%s/depth/%dd.png", path.c_str(), i), cv::IMREAD_ANYDEPTH);
        // cv::Affine3f pose = cv::Affine3f::Identity(); // dt.pose.getvectorPose(i);
        ptsdf->addScan(depth, rgb, pose.inv());
        // // //显示当前TSDF
        // Mat cpu_cloud2;
        // Mat cpu_color;
        // ptsdf->exportCloud(cpu_cloud2, cpu_color);

        // // 显示当前帧深度图对应的点云
        // Mat depth_color, cpu_cloud;
        //  ptsdf->depth2cam(depth, rgb, depth_color, cpu_color, cv::Affine3f::Identity());
        // cv::Affine3f viewpose = cv::Affine3f::Identity();
        // viz1.showWidget("depth", cv::viz::WCloud(depth_color), pose); //, viewpose.translate(cv::Vec3f(4, 0, 0))

        // raycast
        Mat rdp, norm;
        ptsdf->rayCast(rdp, norm, pose.inv());

        cv::imshow("raycast", rdp);
        cv::imshow("norm", norm);
        cv::imshow("rgb", rgb);

        // cv::imwrite(cv::format("pic/norm%d.png",i), norm);
        // cv::imwrite(cv::format("pic/raycast%d.png",i), rdp);

        // cv::cvtColor(norm, norm, cv::COLOR_BGRA2BGR);
        // cv::cvtColor(rdp, rdp, cv::COLOR_BGRA2BGR);
        // cv::Mat vdep;
        // cv::cvtColor(ptsdf->leghtdep, vdep, cv::COLOR_BGRA2BGR);

        if (cv::waitKey(20) == 27)
        {
            break;
        }

        //  ptsdf->save();
        cv::waitKey(5);
        // viz1.spin();
    }
    while (1)
    {
        Mat rdp, norm;
        // cv::imshow("raycast", rdp);
        // cv::imshow("norm", norm);
        char key = cv::waitKey(15);
        if (key == 'n')
            break;
    }
    Mat cpu_cloud2;
    Mat cpu_color;
    ptsdf->exportCloud(cpu_cloud2, cpu_color);
    SaveVoxelGrid2SurfacePointCloud(cpu_cloud2, cpu_color, "tsdf.ply");
}

void kin::run(const cv::Mat &rgb, const cv::Mat &depth, cv::Affine3f pose)
{
    if (i >= paths_rgbs.size())
        return;
    // cout << depth << endl;
    // cv::imshow("depth", depth);
    // cv::imshow("rgb", rgb);
    // waitKey();
    // cv::Affine3f pose(pfsss + i * 16);
    // cout << pose.matrix << endl;
    // 读取图片
    // cv::Mat rgb = cv::imread(cv::format("%s/rgb/%d.png", path.c_str(), i));
    // cv::Mat depth2 = cv::imread(paths_deps[i * 2], cv::IMREAD_ANYDEPTH);
    // cv::Affine3f pose = cv::Affine3f::Identity(); // dt.pose.getvectorPose(i);
    ptsdf->addScan(depth, rgb, pose.inv());
    // // //显示当前TSDF
    // Mat cpu_cloud2;
    // Mat cpu_color;
    // ptsdf->exportCloud(cpu_cloud2, cpu_color);

    // // 显示当前帧深度图对应的点云
    // Mat depth_color, cpu_cloud;
    //  ptsdf->depth2cam(depth, rgb, depth_color, cpu_color, cv::Affine3f::Identity());
    // cv::Affine3f viewpose = cv::Affine3f::Identity();
    // viz1.showWidget("depth", cv::viz::WCloud(depth_color), pose); //, viewpose.translate(cv::Vec3f(4, 0, 0))

    // raycast
    ptsdf->rayCast(rdp, norm, pose.inv());

    // cv::imshow("raycast", rdp);
    // cv::imshow("norm", norm);
    // cv::imshow("rgb", rgb);

    cv::cvtColor(norm, norm, cv::COLOR_BGRA2BGR);
    cv::cvtColor(rdp, rdp, cv::COLOR_BGRA2BGR);
    cv::cvtColor(ptsdf->leghtdep, vdep, cv::COLOR_BGRA2BGR);

    i++;
    cout << "a" << endl;
    // cv::waitKey(0);
    // if ( == 27)
    // {
    // }
}
kin::~kin()
{
}
